#include "segmentDifferences.h"
#include "registration.h"
using namespace std;

vector<PointIndices> findClusters(PointCloud<PointXYZRGB>::Ptr cloud,double tolerance, int min, int max)
{
    vector<PointIndices> clusters;
    EuclideanClusterExtraction<PointXYZRGB> ec;
    ec.setClusterTolerance (tolerance);
    ec.setMinClusterSize (min);
    ec.setMaxClusterSize (max);
    ec.setInputCloud (cloud);
    ec.extract (clusters);
    return clusters;
}

int findWrongClusters(PointCloud<PointXYZRGB>::Ptr registeredCloud,PointCloud<PointXYZRGB>::Ptr targetCloud,
                      PointCloud<PointXYZRGB>::Ptr diffCloud, float scoreThreshold, float distanceThreshold)
{
    time_t start,end;
    time (&start);
    cout << "Difference segmentation cluster-to-cluster... " << flush;
    vector<PointIndices> clusters = findClusters(targetCloud, 1.5, 100, targetCloud->size());
    int nErrors1 = 0, nErrors2 = 0;
    diffCloud->clear();
    PointCloud<PointXYZRGB>::Ptr cloudClustersOk(new PointCloud<PointXYZRGB>);
    for (int i=0; i < clusters.size(); i++)
    {
        PointCloud<PointXYZRGB>::Ptr clusterCloud(new PointCloud<PointXYZRGB>);
        copyPointCloud(*targetCloud,clusters.at(i).indices,*clusterCloud);
        float score = validationScoreProximityTarget<PointXYZRGB>(*registeredCloud,*clusterCloud);
        if (score> scoreThreshold)
        {
            *diffCloud+=*clusterCloud;
            //cout  <<endl<<"Score cluster "<< i <<": "<<score<< " <--------- KO!!";
            nErrors1++;
        }
        else
        {
            *cloudClustersOk+=*clusterCloud;
        }
    }
    //cloudClustersOkcontiene i cluster della target considerati ok anche nella registered
    cout<<"OK! Errors1: "<< nErrors1<<endl;
    //----------------------------------------------
    PointCloud<PointXYZRGB>::Ptr differenceCloud(new PointCloud<PointXYZRGB>);
    cout<<"Difference segmentation point-to-point... "<<flush;
    SegmentDifferences<PointXYZRGB> p;
    //search::KdTree<PointXYZRGB>::Ptr search_tree( new search::KdTree<PointXYZRGB>() );
    p.setInputCloud (cloudClustersOk);//se prendo la registered mette i doppioni!!
    //p.setSearchMethod(search_tree);
    p.setTargetCloud (registeredCloud);//prima target
    p.setDistanceThreshold (distanceThreshold);
    p.segment(*differenceCloud);
    vector<PointIndices> clusters2 = findClusters(differenceCloud,1.5,100,differenceCloud->size());
    for (int i=0;i<clusters2.size();i++)
    {
        PointCloud<PointXYZRGB>::Ptr clusterCloud(new PointCloud<PointXYZRGB>);
        copyPointCloud(*differenceCloud,clusters2.at(i).indices,*clusterCloud);
        *diffCloud+=*clusterCloud;
        //cout << i << " Cluster <--------- KO!!";
        nErrors2++;
    }
    cout<<"OK! Errors: "<< nErrors2<<endl;
    //diffcloud contiene pezzi di target che sono in errore nella registered
    time (&end);
    double dif = difftime (end,start);
    int nErrors=nErrors1+nErrors2;
    cout << "------------------------------------->  Found "<< nErrors <<" missing clusters in " << dif << " seconds"<< endl;

    return nErrors;
}
